/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    RobotLauncher.cpp
  * @author  Mentos Seetoo
  * @brief   THis file includes all modules and start the program, using type 
  *          'float'.			
  * @note    
  * @warning 
  *     - At least C++11 is required.														 
  ******************************************************************************
  * @attention
  * 
  * if you had modified this file, please make sure there is no any bug in your
  * code. Update the version Number and write dowm your name and the date.What 
  * most important is that make sure the users will have a clear understanding
  * through your description.
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "RobotLauncher.h"
#include "matplotlibcpp.h"
#include <gtk/gtk.h>
/* Private define ------------------------------------------------------------*/
namespace plt = matplotlibcpp;

template <typename T>
void setUpKernelParam(QuadrupedKernel<T> *p_kernel);

static void activate(GtkApplication* app, gpointer user_data)
{
    GtkWidget *window;

    window = gtk_application_window_new (app);
    gtk_window_set_title (GTK_WINDOW (window), "Window");
    gtk_window_set_default_size (GTK_WINDOW (window), 200, 200);
    gtk_widget_show_all (window);
}

LogFilter_t Filters[4];//Scheduler, Sim, FSM, Controller

/* Functions -----------------------------------------------------------------*/
/**
 * @brief Construct a new Robot Launcher, do a basical config.
 * 
 */
void RobotLauncher::LaunchAllProgram()
{
    /*
        System Logger tool init.
    */
    std::cout << "[System Logger] Configuring... \n";
    SysLog->getMilliTick_regist(getLinux_runtime);
    SysLog->global_conf(_WARN_, false);
    SysLog->filter_conf(DEFAULT_TAG, LOG_LVL_ALL, LogConsole_Output);
    SysLog->filter_conf("Scheduler", _INFO_, LogConsole_Output, &Filters[0]);
    SysLog->filter_conf("SIM", _INFO_, LogConsole_Output, &Filters[1]);
    SysLog->filter_conf("FSM", _INFO_, LogConsole_Output, &Filters[2]);
    SysLog->filter_conf("Controller", _INFO_, LogConsole_Output, &Filters[3]);
    std::cout << "[System Logger] Logger is ready to be used ! \n";

    /*
        Kernel interfaces init.
    */
    SimInterface::start();

    /*
        Kernel init
    */
    setUpKernelParam(&kernel);
    kernel.start();

    /*
        Display Status
    */
    CThread::printAllThread();
}

#ifdef LOCAL_SIMULATION
/**
 * @brief User can config simulation client in this function.
 *        It's called before coppeliaSim service run.
 */
void RobotLauncher::_configSimulation()
{
    _sim_data.Body = hClient->Add_Object("Body", OTHER_OBJECT, {SIM_POSITION | CLIENT_RO, SIM_VELOCITY | CLIENT_RO});
    _sim_data.Joint[LF][Abduction] = hClient->Add_Object("Shoulder_Joint0", JOINT, {SIM_POSITION | CLIENT_RW}); //SIM_FORCE | CLIENT_RO
    _sim_data.Joint[RF][Abduction] = hClient->Add_Object("Shoulder_Joint1", JOINT, {SIM_POSITION | CLIENT_RW});
    _sim_data.Joint[RB][Abduction] = hClient->Add_Object("Shoulder_Joint2", JOINT, {SIM_POSITION | CLIENT_RW});
    _sim_data.Joint[LB][Abduction] = hClient->Add_Object("Shoulder_Joint3", JOINT, {SIM_POSITION | CLIENT_RW});
    _sim_data.Joint[LF][Hip] = hClient->Add_Object("Hip_Joint0", JOINT, {SIM_POSITION | CLIENT_RW});
    _sim_data.Joint[RF][Hip] = hClient->Add_Object("Hip_Joint1", JOINT, {SIM_POSITION | CLIENT_RW});
    _sim_data.Joint[RB][Hip] = hClient->Add_Object("Hip_Joint2", JOINT, {SIM_POSITION | CLIENT_RW});
    _sim_data.Joint[LB][Hip] = hClient->Add_Object("Hip_Joint3", JOINT, {SIM_POSITION | CLIENT_RW});
    _sim_data.Joint[LF][Knee] = hClient->Add_Object("Knee_Joint0", JOINT, {SIM_POSITION | CLIENT_RW});
    _sim_data.Joint[RF][Knee] = hClient->Add_Object("Knee_Joint1", JOINT, {SIM_POSITION | CLIENT_RW});
    _sim_data.Joint[RB][Knee] = hClient->Add_Object("Knee_Joint2", JOINT, {SIM_POSITION | CLIENT_RW});
    _sim_data.Joint[LB][Knee] = hClient->Add_Object("Knee_Joint3", JOINT, {SIM_POSITION | CLIENT_RW});
    _sim_data.Gyro[Roll] = hClient->Add_Object("Tiger.RollAng", SIM_FLOAT_SIGNAL, {SIM_SIGNAL_OP | CLIENT_RO});
    _sim_data.Gyro[Pitch] = hClient->Add_Object("Tiger.PitchAng", SIM_FLOAT_SIGNAL, {SIM_SIGNAL_OP | CLIENT_RO});
    _sim_data.Gyro[Yaw] = hClient->Add_Object("Tiger.YawAng", SIM_FLOAT_SIGNAL, {SIM_SIGNAL_OP | CLIENT_RO});
}

/**
 * @brief User can set/read the variable value that will be send to/read from
 *        simulation.  
 */
void RobotLauncher::_sendToSimulation()
{
    static bool isFirstData = true;

    if(isFirstData)
    {
        //First data is not send.
        isFirstData = false;
    }
    else
    {
        /*------- RF -----*/
        _sim_data.Joint[RF][Abduction]->obj_Target.angle_f =
        kernel._LegCommand[RF].q(Abduction) + M_PI;
        //kernel._Dynamic.LegState[RF].q(Abduction) + M_PI_2;

        _sim_data.Joint[RF][Hip]->obj_Target.angle_f =
            -(kernel._LegCommand[RF].q(Hip) - M_PI);
        //-(kernel._Dynamic.LegState[RF].q(Hip) - M_PI);

        _sim_data.Joint[RF][Knee]->obj_Target.angle_f =
            -kernel._LegCommand[RF].q(Knee);
        //-kernel._Dynamic.LegState[RF].q(Knee);

        /*------- LF -----*/
        _sim_data.Joint[LF][Abduction]->obj_Target.angle_f =
            kernel._LegCommand[LF].q(Abduction);
        //(kernel._Dynamic.LegState[LF].q(Abduction) - M_PI_2);

        _sim_data.Joint[LF][Hip]->obj_Target.angle_f =
            (kernel._LegCommand[LF].q(Hip) - M_PI);
        //(kernel._Dynamic.LegState[LF].q(Hip) - M_PI);

        _sim_data.Joint[LF][Knee]->obj_Target.angle_f =
            kernel._LegCommand[LF].q(Knee);
        //kernel._Dynamic.LegState[LF].q(Knee);

        /*------- LB -----*/
        _sim_data.Joint[LB][Abduction]->obj_Target.angle_f =
            -kernel._LegCommand[LB].q(Abduction);
        //-(kernel._Dynamic.LegState[LB].q(Abduction) - M_PI_2);

        _sim_data.Joint[LB][Hip]->obj_Target.angle_f =
            (kernel._LegCommand[LB].q(Hip) - M_PI);
        //(kernel._Dynamic.LegState[LB].q(Hip) - M_PI);

        _sim_data.Joint[LB][Knee]->obj_Target.angle_f =
            kernel._LegCommand[LB].q(Knee);
        //kernel._Dynamic.LegState[LB].q(Knee);

        /*------- RB -----*/
        _sim_data.Joint[RB][Abduction]->obj_Target.angle_f =
        - (kernel._LegCommand[RB].q(Abduction) + M_PI);
        //-(kernel._Dynamic.LegState[RB].q(Abduction) + M_PI_2);

        _sim_data.Joint[RB][Hip]->obj_Target.angle_f =
            -(kernel._LegCommand[RB].q(Hip) - M_PI);
        //-(kernel._Dynamic.LegState[RB].q(Hip) - M_PI);

        _sim_data.Joint[RB][Knee]->obj_Target.angle_f =
            -kernel._LegCommand[RB].q(Knee);
        //-kernel._Dynamic.LegState[RB].q(Knee);

        for(int leg(0); leg<4;leg ++)
        {
            //kernel._LegCommand[leg] = kernel._Dynamic.LegState[leg];
            SysLog->Record(_INFO_,"SIM","Joint cmd %d:%f, %f, %f", leg,
                            _sim_data.Joint[leg][Abduction]->obj_Target.angle_f,
                            _sim_data.Joint[leg][Hip]->obj_Target.angle_f,
                            _sim_data.Joint[leg][Knee]->obj_Target.angle_f);
                            // kernel._Dynamic.LegCommand[leg].q(Abduction),
                            // kernel._Dynamic.LegCommand[leg].q(Hip),
                            // kernel._Dynamic.LegCommand[leg].q(Knee));
        }
    }
}

void RobotLauncher::_readFromSimulation()
{
    /*------- RF -----*/
    kernel._Dynamic.LegState[RF].q(Abduction) =
        _sim_data.Joint[RF][Abduction]->obj_Data.angle_f;

    kernel._Dynamic.LegState[RF].q(Hip) =
        -_sim_data.Joint[RF][Hip]->obj_Data.angle_f + M_PI;

    kernel._Dynamic.LegState[RF].q(Knee) =
        -_sim_data.Joint[RF][Knee]->obj_Data.angle_f;
    /*------- LF -----*/
    kernel._Dynamic.LegState[LF].q(Abduction) =
         _sim_data.Joint[LF][Abduction]->obj_Data.angle_f;

    kernel._Dynamic.LegState[LF].q(Hip) =
        _sim_data.Joint[LF][Hip]->obj_Data.angle_f + M_PI;

    kernel._Dynamic.LegState[LF].q(Knee) =
        _sim_data.Joint[LF][Knee]->obj_Data.angle_f;
    /*------- LB -----*/
    kernel._Dynamic.LegState[LB].q(Abduction) =
       - _sim_data.Joint[LB][Abduction]->obj_Data.angle_f;

    kernel._Dynamic.LegState[LB].q(Hip) =
        _sim_data.Joint[LB][Hip]->obj_Data.angle_f + M_PI;

    kernel._Dynamic.LegState[LB].q(Knee) =
        _sim_data.Joint[LB][Knee]->obj_Data.angle_f;
    /*------- RB -----*/
    kernel._Dynamic.LegState[RB].q(Abduction) =
       - _sim_data.Joint[RB][Abduction]->obj_Data.angle_f;

    kernel._Dynamic.LegState[RB].q(Hip) =
        -_sim_data.Joint[RB][Hip]->obj_Data.angle_f + M_PI;

    kernel._Dynamic.LegState[RB].q(Knee) =
        -_sim_data.Joint[RB][Knee]->obj_Data.angle_f;

    /*RPY angles*/
    // kernel._Dynamic.BodyState.rpy(Roll) = _sim_data.Gyro[Roll]->data;
    // kernel._Dynamic.BodyState.rpy(Pitch) = _sim_data.Gyro[Pitch]->data;
    // kernel._Dynamic.BodyState.rpy(Yaw) = _sim_data.Gyro[Yaw]->data;

    /*Body states（注意M系和仿真坐标系G的关系）*/
    kernel._StateEstimator.rpy(Roll) = _sim_data.Gyro[Roll]->data;
    kernel._StateEstimator.rpy(Pitch) = _sim_data.Gyro[Pitch]->data;
    kernel._StateEstimator.rpy(Yaw) = _sim_data.Gyro[Yaw]->data;

    kernel._StateEstimator.p(0) = _sim_data.Body->obj_Data.position_3f[1];
    kernel._StateEstimator.p(1) = -_sim_data.Body->obj_Data.position_3f[0];
    kernel._StateEstimator.p(2) = _sim_data.Body->obj_Data.position_3f[2];

    kernel._StateEstimator.v(0) = _sim_data.Body->obj_Data.linVelocity_3f[1];
    kernel._StateEstimator.v(1) = -_sim_data.Body->obj_Data.linVelocity_3f[0];
    kernel._StateEstimator.v(2) = _sim_data.Body->obj_Data.linVelocity_3f[2];

    kernel._StateEstimator.w(0) = _sim_data.Body->obj_Data.angVelocity_3f[2];
    kernel._StateEstimator.w(1) = _sim_data.Body->obj_Data.angVelocity_3f[0];
    kernel._StateEstimator.w(2) = _sim_data.Body->obj_Data.angVelocity_3f[1];


    for(int leg(0); leg<4;leg ++)
    {
        SysLog->Record(_INFO_,"SIM","Joint angle %d:%f, %f, %f", leg,
                        // _sim_data.Joint[leg][Abduction]->obj_Data.angle_f,
                        // _sim_data.Joint[leg][Hip]->obj_Data.angle_f,
                        // _sim_data.Joint[leg][Knee]->obj_Data.angle_f);
                        kernel._Dynamic.LegState[leg].q(Abduction),
                        kernel._Dynamic.LegState[leg].q(Hip),
                        kernel._Dynamic.LegState[leg].q(Knee));
    }
    SysLog->Record(_INFO_,"SIM","CoM speed:%f, %f, %f",
                    kernel._StateEstimator.v(0),
                    kernel._StateEstimator.v(1),
                    kernel._StateEstimator.v(2)); 
    SysLog->Record(_INFO_,"SIM","CoM rotation speed:%f, %f, %f",
                    kernel._StateEstimator.w(0),
                    kernel._StateEstimator.w(1),
                    kernel._StateEstimator.w(2));
    SysLog->Record(_INFO_,"SIM","CoM position:%f, %f, %f",
                    kernel._StateEstimator.p(0),
                    kernel._StateEstimator.p(1),
                    kernel._StateEstimator.p(2)); 
}
#else
void RobotLauncher::_configSimulation(){};
void RobotLauncher::_sendToSimulation(){};
void RobotLauncher::_readFromSimulation(){};
#endif

template <typename T>
void setUpKernelParam(QuadrupedKernel<T> *p_kernel)
{
    /*
        Dynamic parameters
    */
    p_kernel->_Dynamic.Param.BODY_MASS = 4.5;
    p_kernel->_Dynamic.Param.BODY_LENGTH = 0.19 * 2;
    p_kernel->_Dynamic.Param.BODY_WIDTH = 0.049 * 2;
    p_kernel->_Dynamic.Param.BODY_HEIGHT = 0.05 * 2;
    p_kernel->_Dynamic.Param.ABAD_GEAR_RATIO = 6;
    p_kernel->_Dynamic.Param.HIP_GEAR_RATIO = 6;
    p_kernel->_Dynamic.Param.KNEE_GEAR_RATIO = 9.33;
    p_kernel->_Dynamic.Param.ABAD_LINK_LEN = 0.062;
    p_kernel->_Dynamic.Param.HIP_LINK_LEN = 0.209;
    p_kernel->_Dynamic.Param.KNEE_LINK_LEN = 0.195;
    p_kernel->_Dynamic.Param.KNEE_Y_OFFSET = 0.00;
    p_kernel->_Dynamic.Param.MAX_LEG_LEN = 0.409;

    p_kernel->_Dynamic.Param.MOTOR_MAX_TORQUE = 3.f;
    p_kernel->_Dynamic.Param.SUPPLY_VOLTAGE = 24;
    p_kernel->_Dynamic.Param.MOTOR_KT = 0.05;
    p_kernel->_Dynamic.Param.MOTOR_R = 0.173;
    p_kernel->_Dynamic.Param.JOINT_DAMP = 0.01;
    p_kernel->_Dynamic.Param.JOINT_FRICTION = 2;

    // rotor inertia if the rotor is oriented so it spins around the z-axis
    Mat3<T> rotorRotationalInertiaZ;
    rotorRotationalInertiaZ << 33, 0, 0, 0, 33, 0, 0, 0, 63;
    rotorRotationalInertiaZ = 1e-6 * rotorRotationalInertiaZ;

    Mat3<T> RY = coordinateRotation<T>(CoordinateAxis::Y, M_PI / 2);
    Mat3<T> RX = coordinateRotation<T>(CoordinateAxis::X, M_PI / 2);
    Mat3<T> rotorRotationalInertiaX =
        RY * rotorRotationalInertiaZ * RY.transpose();
    Mat3<T> rotorRotationalInertiaY =
        RX * rotorRotationalInertiaZ * RX.transpose();

    // spatial inertias
    Mat3<T> abadRotationalInertia;
    abadRotationalInertia << 381, 58, 0.45, 58, 560, 0.95, 0.45, 0.95, 444;
    abadRotationalInertia = abadRotationalInertia * 1e-6;
    Vec3<T> abadCOM(0, 0.036, 0); // LEFT
    SpatialInertia<T> abadInertia(0.54, abadCOM, abadRotationalInertia);

    Mat3<T> hipRotationalInertia;
    hipRotationalInertia << 1983, 245, 13, 245, 2103, 1.5, 13, 1.5, 408;
    hipRotationalInertia = hipRotationalInertia * 1e-6;
    Vec3<T> hipCOM(0, 0.016, -0.02);
    SpatialInertia<T> hipInertia(0.634, hipCOM, hipRotationalInertia);

    Mat3<T> kneeRotationalInertia, kneeRotationalInertiaRotated;
    kneeRotationalInertiaRotated << 6, 0, 0, 0, 248, 0, 0, 0, 245;
    kneeRotationalInertiaRotated = kneeRotationalInertiaRotated * 1e-6;
    kneeRotationalInertia = RY * kneeRotationalInertiaRotated * RY.transpose();
    Vec3<T> kneeCOM(0, 0, -0.061);
    SpatialInertia<T> kneeInertia(0.064, kneeCOM, kneeRotationalInertia);

    Vec3<T> rotorCOM(0, 0, 0);
    SpatialInertia<T> rotorInertiaX(0.055, rotorCOM, rotorRotationalInertiaX);
    SpatialInertia<T> rotorInertiaY(0.055, rotorCOM, rotorRotationalInertiaY);

    Mat3<T> bodyRotationalInertia;
    bodyRotationalInertia << 11253, 0, 0, 0, 36203, 0, 0, 0, 42673;
    bodyRotationalInertia = bodyRotationalInertia * 1e-6;
    Vec3<T> bodyCOM(0, 0, 0);
    SpatialInertia<T> bodyInertia(p_kernel->_Dynamic.Param.BODY_MASS, bodyCOM,
                                  bodyRotationalInertia);

    p_kernel->_Dynamic._abadInertia = abadInertia;
    p_kernel->_Dynamic._hipInertia = hipInertia;
    p_kernel->_Dynamic._kneeInertia = kneeInertia;
    p_kernel->_Dynamic._abadRotorInertia = rotorInertiaX;
    p_kernel->_Dynamic._hipRotorInertia = rotorInertiaY;
    p_kernel->_Dynamic._kneeRotorInertia = rotorInertiaY;
    p_kernel->_Dynamic._bodyInertia = bodyInertia;

    p_kernel->_Dynamic._abadRotorLocation = Vec3<T>(0.125, 0.049, 0);
    p_kernel->_Dynamic._abadLocation =
        Vec3<T>(p_kernel->_Dynamic.Param.BODY_LENGTH, p_kernel->_Dynamic.Param.BODY_WIDTH, 0) * 0.5;
    p_kernel->_Dynamic._hipLocation = Vec3<T>(0, p_kernel->_Dynamic.Param.ABAD_LINK_LEN, 0);
    p_kernel->_Dynamic._hipRotorLocation = Vec3<T>(0, 0.04, 0);
    p_kernel->_Dynamic._kneeLocation = Vec3<T>(0, 0, -p_kernel->_Dynamic.Param.HIP_LINK_LEN);
    p_kernel->_Dynamic._kneeRotorLocation = Vec3<T>(0, 0, 0);

    /*
        Command parameters.
    */
    struct DesiredConstrain<T> Dolphin_Constrain = {
        3, -3,
        3, -3,
        3 ,-3,
        3,
        30, -30,
        30, -30,
        30, -30,
        0.4, 0.1,
        0.01
    };

    p_kernel->_CoMCommand.SetConstrains(Dolphin_Constrain);
}
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/